within ThreeD_MBS_Dynamics.Examples.OmniVehicle.PatchContact;

partial model RollerContactVelocities
  extends RollerContactTracking;
  Real kappa;
  Real[3] PA;
  Real[3] PB;
  Real[3] vPA;
  Real[3] vPB;
  Real[3] relv;
  Real relvn;
  Real vPAn;
  Real vPBn;
  Real[3] vPAt;
  Real[3] vPBt;
  Real[3] relvt;
  Real relvtsqrt;
equation
  kappa = rB[2];

  if noEvent(kappa <= 0) then
    PA = (rB + rA)/2;
    PB = PA;
  else
    PA = rA;
    PB = rB;
  end if;
  vPA = InPortA.v + cross(InPortA.omega, PA - InPortA.r);
  vPB = InPortB.v + cross(InPortB.omega, PB - InPortB.r);
  relv = vPB - vPA;
  relvn = relv*nA;
  vPAn = vPA*nA;
  vPBn = vPB*nA;
  vPAt = vPA - vPAn*nA;
  vPBt = vPB - vPBn*nA;
  relvt = vPBt - vPAt;
  relvtsqrt = sqrt(relvt*relvt);

  OutPortA.P = PA;
  OutPortB.P = PB;

end RollerContactVelocities;
